/*
      ____                      _____                  +---+
     / ___\                     / __ \                 | R |
    / /                        / /_/ /                 +---+
   / /   ________  ____  ___  / ____/___  ____  __   __
  / /  / ___/ __ `/_  / / _ \/ /   / __ \/ _  \/ /  / /
 / /__/ /  / /_/ / / /_/  __/ /   / /_/ / / / / /__/ /
 \___/_/   \__,_/ /___/\___/_/    \___ /_/ /_/____  /
                                                 / /
                                            ____/ /
                                           /_____/
main.c file
编写者：小马  (Camel)
作者E-mail：375836945@qq.com
编译环境：MDK-Lite  Version: 4.23
初版时间: 2014-01-28
功能：
------------------------------------
*/
#include "config.h"             //包含所有的驱动头文件
#include "Led.h"
#include "Pwm.h"
#define UART_DEBUG

#define DHT11_OUT PBout(5)
#define DHT11_IN PBin(5)

#define FORWARD 0
#define BACK 1
#define STOP 2

#define LEFTA PBout(12)
#define LEFTB PBout(13)
#define RIGHTA PBout(14)
#define RIGHTB PBout(15)

#define SPEEDL PAout(0)
#define SPEEDR PAout(1)


#define Moto_PwmMax 999
int16_t MOTO1_PWM = 0;
int16_t MOTO2_PWM = 0;

#define PB9_IN  PBin(9)
#define PB8_OUT PBout(8)

#define UP_DOWN_SPEED 20
#define ONE_UP_DOWN 7
#define SPEED_DEVALUE 100
#define R_L_DEVALUE 5



int left_speed = (308-1);
int right_speed = (310-1);




void Left_Speed_Set(unsigned int pulse);
void Right_Speed_Set(unsigned int pulse);
void Left_Motor(int mode);
void Right_Motor(int mode);

char cmd[] = {'G', 'B', 'S', 'L', 'R', 'U', 'D'};

void wheel_go(void){
	Left_Motor(FORWARD);
	Right_Motor(FORWARD);
	
	left_speed = 350 - 1;
	right_speed = 337 - 1;
	
	if(right_speed - left_speed > SPEED_DEVALUE)
		left_speed = right_speed - R_L_DEVALUE;
	else if(left_speed - right_speed > SPEED_DEVALUE)
		right_speed = left_speed + R_L_DEVALUE;
	Left_Speed_Set(left_speed);
	Right_Speed_Set(right_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}

void wheel_back(void){
	//GPIO_SetBits(GPIOB, GPIO_Pin_13|GPIO_Pin_15);
	//GPIO_ResetBits(GPIOB, GPIO_Pin_12|GPIO_Pin_14);

	Left_Motor(BACK);
	Right_Motor(BACK);
	
	left_speed = 350 - 1;
	right_speed = 337 - 1;
	
	if(right_speed - left_speed > SPEED_DEVALUE)
		left_speed = right_speed - R_L_DEVALUE;
	else if(left_speed - right_speed > SPEED_DEVALUE)
		right_speed = left_speed + R_L_DEVALUE;
	
	Left_Speed_Set(left_speed);
	Right_Speed_Set(right_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}

void wheel_stop(void){
	GPIO_ResetBits(GPIOB, GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15);
	printf("left:%d, right:%d\n", left_speed, right_speed);
}

void wheel_turn_left(void){
	GPIO_ResetBits(GPIOB, GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_15);
//	GPIO_ResetBits(GPIOB, GPIO_Pin_13);
	GPIO_SetBits(GPIOB, GPIO_Pin_14);
//	GPIO_ResetBits(GPIOB, GPIO_Pin_15);
	left_speed = 0;//问题：左转速度过快，导致原地转圈：1、重新控制速度；2、采取PID算法将速度控制稳定
	Left_Speed_Set(left_speed);
	Right_Speed_Set(right_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}

void wheel_turn_right(void){
	GPIO_SetBits(GPIOB, GPIO_Pin_12);
	GPIO_ResetBits(GPIOB, GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15);
//	GPIO_ResetBits(GPIOB, GPIO_Pin_14);
//	GPIO_ResetBits(GPIOB, GPIO_Pin_15);
	right_speed = 0;//问题：右转速度过快，导致原地转圈
	Left_Speed_Set(left_speed);
	Right_Speed_Set(right_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}

void wheel_up(void){
	left_speed += UP_DOWN_SPEED;
	right_speed += UP_DOWN_SPEED;
	Left_Speed_Set(left_speed);
	Right_Speed_Set(right_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}


void wheel_down(void){
	if(left_speed > UP_DOWN_SPEED)
		left_speed -= UP_DOWN_SPEED;
	if(right_speed > UP_DOWN_SPEED)
		right_speed -= UP_DOWN_SPEED;
	
	Left_Speed_Set(left_speed);
	Right_Speed_Set(right_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}


void wheel_left_up(void){
	left_speed += ONE_UP_DOWN;
	Left_Speed_Set(left_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}


void wheel_left_down(void){
	if(left_speed > ONE_UP_DOWN)
		left_speed -= ONE_UP_DOWN;
	
	Left_Speed_Set(left_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}

void wheel_right_up(void){
	right_speed += ONE_UP_DOWN;
	Right_Speed_Set(right_speed);
	
	printf("left:%d, right:%d\n", left_speed, right_speed);
}

void wheel_right_down(void){
	if(right_speed > ONE_UP_DOWN)
		right_speed -= ONE_UP_DOWN;

	Right_Speed_Set(right_speed);

	printf("left:%d, right:%d\n", left_speed, right_speed);
}



void recv_cmd(void){
	uint8_t temp;
//	char recv_cmd[4];
	uint8_t Cmd[4] = {0};
	if(USART1_bytesAvailable() > 0){
		temp = USART1_Getchar();
		Cmd[0] = temp;
		
	//	UartSendBuffer(&temp, 1);
	}
		switch(Cmd[0]){
			case 'G':wheel_go();break;
			case 'B':wheel_back();break;
			case 'S':wheel_stop();break;
			case 'L':wheel_turn_left();break;
			case 'R':wheel_turn_right();break;
		  case 'U':wheel_up();break;
			case 'D':wheel_down();break;
			case '1':wheel_left_up();break;
			case '3':wheel_left_down();break;
			case '2':wheel_right_up();break;
			case '4':wheel_right_down();break;
			default:break;
		}

}

void PwmFlash(int16_t MOTO1_PWM,int16_t MOTO2_PWM)
{		
     if(MOTO1_PWM>=Moto_PwmMax)	MOTO1_PWM = Moto_PwmMax;
     if(MOTO2_PWM>=Moto_PwmMax)	MOTO2_PWM = Moto_PwmMax;
     
     if(MOTO1_PWM<=0)	MOTO1_PWM = 0;
     if(MOTO2_PWM<=0)	MOTO2_PWM = 0;//限定输入不能小于0，大于999
    
    TIM2->CCR1 = MOTO1_PWM;
    TIM2->CCR2 = MOTO2_PWM;      //对定时器寄存器赋值
}


void PwmInit(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    
    
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //打开外设A的时钟和复用时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 ,ENABLE);   //打开定时器2时钟  
    
    
    // 设置GPIO功能。
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//2MHZ
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    
    
    // 复位定时器。
    TIM_DeInit(TIM2);
	/******************/
	TIM_InternalClockConfig(TIM2);
    
    // 配置计时器。
    //PrescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
    
    TIM_TimeBaseStructure.TIM_Period = Moto_PwmMax;		            //计数上线	
    TIM_TimeBaseStructure.TIM_Prescaler = 72;	//pwm时钟分频
	//TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;	//pwm时钟分频
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	//TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数
    
    TIM_TimeBaseInit(TIM2,&TIM_TimeBaseStructure);

	
	TIM_ARRPreloadConfig(TIM2, DISABLE);
	
	TIM_Cmd(TIM2,ENABLE);
}

void Left_Speed_Set(unsigned int pulse)
{
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	
	// 配置TIM2为PWM输出模式
    TIM_OCStructInit(&TIM_OCInitStructure);
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = pulse;    //0
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    
    TIM_OC1Init(TIM2,&TIM_OCInitStructure);
   
	TIM_CtrlPWMOutputs(TIM2, ENABLE);
   
  //  printf("Motor init success...\r\n");
}

void Right_Speed_Set(unsigned int pulse)
{
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	
	// 配置TIM2为PWM输出模式
    TIM_OCStructInit(&TIM_OCInitStructure);
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = pulse;    //0
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

    TIM_OC2Init(TIM2,&TIM_OCInitStructure);

	TIM_CtrlPWMOutputs(TIM2, ENABLE);
   
   // printf("Motor init success...\r\n");
}

void Port_Init()
{
	GPIO_InitTypeDef GPIO_InitStructure;
		
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);	
	//GPIO_ResetBits(GPIOB, GPIO_Pin_5);//用于初始化	
	GPIO_SetBits(GPIOB, GPIO_Pin_12| GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);//用于初始化
}

void PortB9IN(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;
	//Configure pin as input 
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 ;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 	//浮动输入
	GPIO_Init(GPIOB,&GPIO_InitStructure);
}
void PortB8OUT(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;

	//Configure pin as input 
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 ;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 	//推挽输出
	GPIO_Init(GPIOB,&GPIO_InitStructure);
}

void Left_Motor(int mode)
{
	switch(mode)
	{
		case FORWARD :
		{
			LEFTA = 1;
			LEFTB = 0;
			break;
		}
		case BACK :
		{
			LEFTA = 0;
			LEFTB = 1;
			break;
		}
		case STOP :
		{
			LEFTA = 0;
			LEFTB = 0;
			break;
		}
		default : break;
	}
}	



void Right_Motor(int mode)
{
	switch(mode)
	{
		case FORWARD :
		{
			RIGHTA = 1;
			RIGHTB = 0;
			break;
		}
		case BACK :
		{
			RIGHTA = 0;
			RIGHTB = 1;
			break;
		}
		case STOP :
		{
			RIGHTA = 0;
			RIGHTB = 0;
			break;
		}
		default : break;
	}
}	


int get_dis()
{
	int i = 0; 
	float s = 0.0;
	int time = 0;
	for(i = 0;i<5;i++)
	{
		PortB9IN();
		PortB8OUT();
		PB8_OUT = 1;
		delay_us(25);
		PB8_OUT = 0;
		
		while(!PB9_IN);//等到返回的高电平
		while(PB9_IN)
		{
			delay_us(15);
			++time;
		}
		s += (time*15/10000.0)*170;
		delay_ms(1);//重新设置延时，调整测距方程。
		time = 0;
	}
	s = s/5.0;
//	printf("The distance is : %lfcm \r\n ", s);
//	delay_ms(100);
	return (int)s;
}



int main(void)
{
	int dis = 0;
	/***********************************/
	SystemClock_HSI(9);           //系统时钟初始化，时钟源内部HSI
	
	cycleCounterInit();				    // Init cycle counter
	SysTick_Config(SystemCoreClock / 1000);	//SysTick开启系统tick定时器并初始化其中断，1ms
	UART1_init(SysClock,uart1baudSet); //串口1初始化
	NVIC_INIT();	                //中断初始化
	//KeyInit();										//按键初始化
	//LedInit();
	//Adc_Init();
	
	Port_Init();
	PwmInit();
	
#ifdef UART_DEBUG  
	TIM3_Init(SysClock,300);			//定时器初始化，1s为周期打印摇杆值
#endif
	TIM4_Init(SysClock,TIME4_Preiod);	  //定时器4初始化，定时时间单位：(TIME4_Preiod)微秒


	PwmFlash(10, 10);//调整初值，再细分速度；调整占空比。
	Left_Speed_Set(left_speed);
	Right_Speed_Set(right_speed);
		
	Left_Motor(FORWARD);
	Right_Motor(FORWARD);

		
	while (1)             
	{	
		dis = get_dis();
		while(dis < 0)//测距范围改变
		{
			if(dis > 20)
		{
				Left_Motor(FORWARD);
				Right_Motor(STOP);
				Left_Speed_Set(250 - 1);
			}
			else if(dis > 10)
			{
				Left_Motor(FORWARD);
				Right_Motor(BACK);
				//Left_Speed_Set(250 - 1);
				//Right_Speed_Set(250 - 1);
				Left_Speed_Set(270 - 1);
				Right_Speed_Set(270 - 1);
			}
			else
			{
				Left_Motor(BACK);
				Right_Motor(BACK);
				Left_Speed_Set(500 - 1);
				Right_Speed_Set(500 - 1);
			}
			dis = get_dis();
		}
		
		 Left_Speed_Set(left_speed);
		 Right_Speed_Set(right_speed);
		 Left_Motor(FORWARD);
		 Right_Motor(FORWARD);
		 dis = get_dis();
		
		while(dis >= 0)
		{
			recv_cmd();
			dis = get_dis();
		}
	}
}

